#ifndef MOVE_YOUBOT_ARM__CLIENT_H_
#define MOVE_YOUBOT_ARM__CLIENT_H_

#define TESTING_ARM 0

#include "youbot_relay/MoveYoubotArmAction.h"

#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/simple_client_goal_state.h>

#include "ros/ros.h"

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Twist.h>

#include <tf/transform_datatypes.h>

#define CLIENT_NODE_ARM "move_youbot_arm_client"
#define CLIENT_ACTION_ARM "move_youbot_arm"
#define DEFAULT_TIMEOUT_ARM ros::Duration(10, 0)


namespace move_youbot_arm
{
  class MoveYoubotArmClient
  {
  public:
    typedef actionlib::SimpleActionClient<youbot_relay::MoveYoubotArmAction> Client;

    MoveYoubotArmClient(int argc, char** argv);

    //all angles are given in degree -> converting into radians
    //the client asks for the full distance of gripper's jaws, but internally each jaw will be moved by half distance
    bool move( float joint1_degree, float joint2_degree, float joint3_degree, float joint4_degree, float joint5_degree, float gripper_meters=0, ros::Duration timeout=ros::Duration(0, 0));
    bool moveSynchronous( float joint1_degree, float joint2_degree, float joint3_degree, float joint4_degree, float joint5_degree, float gripper_meters=0);//this method is blocking until goal is reached or aborted
    bool move( youbot_relay::MoveYoubotArmGoal g, ros::Duration timeout=ros::Duration(0, 0) );
    bool move( ros::Duration timeout=ros::Duration(0, 0) );
    bool getCurrentPosition( float *joint1, float *joint2, float *joint3, float *joint4, float *joint5, float *gripper );
    actionlib::SimpleClientGoalState getState();
    bool stop();

  private:
    Client *client;
    youbot_relay::MoveYoubotArmGoal goal;


    void setGoal( youbot_relay::MoveYoubotArmGoal g);
    void setGoal(float joint1_degree, float joint2_degree, float joint3_degree, float joint4_degree, float joint5_degree, float gripper_meters=0);
    bool sendGoal();
    actionlib::SimpleClientGoalState sendGoalForResult( ros::Duration timeout = DEFAULT_TIMEOUT_ARM);

  };
}


#endif
